//
// Created by xujingyi on 2021/5/7.
//
#include "find/armor_class.h"
#include "show_images.h"
#include "additions.h"
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
cv::Scalar getColor(rm::Marker marker){
    cv::Scalar color(0, 255, 0);
    if (marker.color() == rm::RED)
        color = cv::Scalar(0, 0, 255);
    else if (marker.color() == rm::BLUE)
        color = cv::Scalar(255, 100, 0);
    else if (marker.color() == rm::PURPLE)
        color = cv::Scalar(200, 0, 200);
    return color;
}

void showMarker(const cv::Mat &src, const rm::Marker &light_blob) {
    cv::Scalar color=getColor(light_blob);
    std::vector<cv::Point2f> vertices=light_blob.vertexes();
    for (int j = 0; j < 4; j++) {
//        cv::circle(src,vertices[j],3,cv::Scalar(50*j,50*j,50*j),-1);
        cv::line(src, vertices[j], vertices[(j + 1) % 4], color, 2);
    }
#ifdef SHOW_PARAM
    char center[100],peri[100],h_w_ratio[100],angle[100],len[100];
    sprintf(center,"(%d,%d)",int(vertices[0].x),int(vertices[0].y));
    sprintf(peri,"%.1f",light_blob.getPeri());
    sprintf(h_w_ratio,"%.1f",light_blob.getHWRatio());
    sprintf(angle,"%.1f",light_blob.angle());
    sprintf(len,"%.1f",light_blob.size().x);
    cv::putText(src,center,cv::Point2i(vertices[0].x,vertices[0].y-50),1,1,color);
    cv::putText(src,"center=",cv::Point2i(vertices[0].x,vertices[0].y-70),1,1,color);
    cv::putText(src,peri,cv::Point2i(vertices[0].x,vertices[0].y-100),1,1,color);
    cv::putText(src,"peri=",cv::Point2i(vertices[0].x,vertices[0].y-120),1,1,color);
    cv::putText(src,h_w_ratio,cv::Point2i(vertices[0].x,vertices[0].y-150),1,1,color);
    cv::putText(src,"h_w_ratio=",cv::Point2i(vertices[0].x,vertices[0].y-170),1,1,color);
    cv::putText(src,angle,cv::Point2i(vertices[0].x,vertices[0].y-200),1,1,color);
    cv::putText(src,"angle=",cv::Point2i(vertices[0].x,vertices[0].y-220),1,1,color);
    cv::putText(src,len,cv::Point2i(vertices[0].x,vertices[0].y-250),1,1,color);
    cv::putText(src,"length=",cv::Point2i(vertices[0].x,vertices[0].y-270),1,1,color);
#endif
};

void showMarkers(const cv::Mat &src, const rm::Markers &markers){
    //for (auto &marker : maybe_enemy_markers) {
    for (auto &marker : markers) {
        showMarker(src, marker);
    }
}

void showMarkerCouple(const cv::Mat &src, const rm::MarkerCouple &couple){
    rm::Marker left=couple.left_mark,right=couple.right_mark;

    cv::Scalar color=getColor(left);
    //left
    std::vector<cv::Point2f> vertices=left.vertexes();
    for (int j = 0; j < 4; j++) {
        cv::line(src, vertices[j], vertices[(j + 1) % 4], color, 2);
    }

#ifdef SHOW_PARAM
    char l_center[100];
    sprintf(l_center,"(%d,%d)",(int)left.center().x,(int)couple.left_mark.center().y);
    cv::putText(src,"L",cv::Point2i(vertices[0].x,vertices[0].y+30),1,1.5,color);
    cv::putText(src,l_center,cv::Point2i(vertices[0].x,vertices[0].y+70),1,1,color);
#endif
    //right
    vertices=right.vertexes();
    for (int j = 0; j < 4; j++) {
        cv::line(src, vertices[j], vertices[(j + 1) % 4], color, 2);
    }

#ifdef SHOW_PARAM
    char r_center[100];
    sprintf(r_center,"(%d,%d)",(int)right.center().x,(int)right.center().y);
    cv::putText(src,"R",cv::Point2i(vertices[0].x,vertices[0].y+30),1,1.5,color);
    cv::putText(src,r_center,cv::Point2i(vertices[0].x,vertices[0].y+70),1,1,color);

    //puttext
    char c1[40],c2[40],c3[40],c4[40];
//    float len_dy_ratio=std::max(left.size().x,right.size().x)/(abs(left.center().y-right.center().y)==0?0.01:abs(left.center().y-right.center().y)) ;
    float lens_ratio = std::max((left.center().x/right.center().x),(right.center().x/left.center().x));
    float angle_diff=abs(left.angle()-right.angle());
    cv::Point2f v=right.vertexes()[0]-left.vertexes()[3];
    float box_angle=(atan2(v.y,v.x)>CV_PI?(2*CV_PI-atan2(v.y,v.x)):(-atan2(v.y,v.x)))*180/CV_PI;
    float marker_box_angle_diff=std::min(abs(left.angle()-box_angle),abs(right.angle()-box_angle));
    float dis_len_ratio=getPointLength(left.center(),right.center())/std::max(left.size().x,right.size().x);
    sprintf(c1,"lens_ratio=%.1f",lens_ratio);
    sprintf(c2,"%.1f",angle_diff);
    sprintf(c3,"%.4f",marker_box_angle_diff);
    sprintf(c4,"%.4f",dis_len_ratio);
    cv::putText(src,c1,cv::Point2i(left.center().x,couple.center().y+100),1,1,color);
    cv::putText(src,"ang_diff=",cv::Point2i(left.center().x,couple.center().y+130),1,1,color);
    cv::putText(src,c2,cv::Point2i(left.center().x,couple.center().y+150),1,1,color);
    cv::putText(src,"mk_bx_ang_diff=",cv::Point2i(left.center().x,couple.center().y+180),1,1,color);
    cv::putText(src,c3,cv::Point2i(left.center().x,couple.center().y+200),1,1,color);
    cv::putText(src,"dis_len_ratio=",cv::Point2i(left.center().x,couple.center().y+230),1,1,color);
    cv::putText(src,c4,cv::Point2i(left.center().x,couple.center().y+250),1,1,color);
    #endif

    vertices=couple.vertexes();
    for (int j = 0; j < 4; j++) {
        cv::line(src, vertices[j], vertices[(j + 1) % 4], cv::Scalar(50*j,50*j,50*j), 2);
        cv::circle(src,vertices[j],3,cv::Scalar(50*j,50*j,50*j),-1);
    }
}

void showMarkerCouples(const cv::Mat &src, const rm::MarkerCouples &couples){
    for (auto &couple:couples) showMarkerCouple(src,couple);
}

void showArmor(const cv::Mat &src, const rm::Armor &armor){
    std::vector<cv::Point2f> vertices=armor.vertexes();
    if (vertices.size()==4) {
        showMarkerCouple(src, armor.couple);

        for (int j = 0; j < 4; j++) {
            cv::line(src, vertices[j], vertices[(j + 1) % 4], COLOR_GREEN/5, 2);
            cv::circle(src, vertices[j], 3, COLOR_WHITE, -1);
        }

        char size[20];
        if (armor.armorSize()==rm::BIG_BOX) sprintf(size,"%s"," BIG");
        else if (armor.armorSize()==rm::SMALL_BOX) sprintf(size,"%s"," SMA");
        else if (armor.armorSize()==rm::NOT_BOX) sprintf(size,"%s"," NO");
//        //rectangle(image2show, box.box_rect, Scalar(0, 255, 0), 3);
//        circle(image2show, cv::Point2d(box.predict_box_c_x, box.predict_box_c_y), 3, Scalar(0, 255, 0), -1);

        cv::Point2f center = armor.center();
        cv::Scalar color;
        if (armor.getID() == -1) color=cv::Scalar(0, 255, 0)/5;
        else if (1 <= armor.getID() && armor.getID() < 8)  color=cv::Scalar(255, 100, 0)/5;
        else if (8 <= armor.getID() && armor.getID() < 15) color=cv::Scalar(0, 0, 255)/5;

//        char c_center[200];
//        sprintf(c_center,"(%d,%d)",(int)center.x,(int)center.y);
//        cv::putText(src,c_center,cv::Point2i(center.x-40,center.y-60),1,1,color);
        putText(src, rm::id2name[armor.getID()]+size,
                cv::Point(armor.couple.center().x + 2, armor.couple.center().y - 10),
                cv::FONT_HERSHEY_TRIPLEX, 1,
                color);

switch (armor.box_pos.getState()){
    case rm::HIGHLAND:
        cv::putText(src,"HIGHLAND",cv::Point2i(center.x-25,center.y+50),1,2,COLOR_WHITE,2);
        break;
    case rm::GROUND:
        cv::putText(src,"GROUND",cv::Point2i(center.x-25,center.y+50),1,2,COLOR_WHITE,2);
        break;
    case rm::UNKNOWN:
        cv::putText(src,"UNKNOWN",cv::Point2i(center.x-25,center.y+50),1,2,COLOR_WHITE,2);
        break;
}
    }
};

void showArmors(const cv::Mat &src, const rm::Armors &armors){
    for (auto &armor:armors) showArmor(src,armor);
};

void drawCross(const cv::Mat &src, const cv::Point2f center, const cv::Scalar color){
    cv::line(src,cv::Point2f(center.x-15,center.y),cv::Point2f(center.x+15,center.y),color,2.5);
    cv::line(src,cv::Point2f(center.x,center.y-15),cv::Point2f(center.x,center.y+15),color,2.5);
};

void showGryo(const cv::Mat &src,rm::GyroTool gyro_tool, const cv::Point2f center, const cv::Scalar color) {
    char data[64];
    sprintf(data, "center: %.2f %.2f %.2f\0", gyro_tool.gyro_center.x, gyro_tool.gyro_center.y, gyro_tool.gyro_center.z);
    cv::putText(src, data, center, cv::FONT_HERSHEY_COMPLEX,0.7,color,2,0);
    sprintf(data, "Omiga: %.2f\0", gyro_tool.omega);
    cv::putText(src, data, center+cv::Point2f(0, 25), cv::FONT_HERSHEY_COMPLEX,0.7,color,2,0);
    switch(gyro_tool.getState()){
        case rm::GyroTool::LEVO:
            cv::putText(src, "rotate: left", center+cv::Point2f(0, 50), cv::FONT_HERSHEY_COMPLEX,0.7,color,2,0);
            break;
        case rm::GyroTool::DEXTRO:
            cv::putText(src, "rotate: right", center+cv::Point2f(0, 50), cv::FONT_HERSHEY_COMPLEX,0.7,color,2,0);
            break;
        case rm::GyroTool::UNKNOWN:
            cv::putText(src, "rotate: unknow", center+cv::Point2f(0, 50), cv::FONT_HERSHEY_COMPLEX,0.7,color,2,0);
            break;
    }
}

void showResult(const cv::Mat &show_pic, const rm::Armor &armor,const cv::Point3f pos_message){
    cv::Scalar color=getColor(armor.couple.left_mark)/5;
    char dist[100], c_pitch[100], c_yaw[100],c_eular[100],c_real[100],c_pre[100];
    sprintf(dist, "distance=%.1f", pos_message.z);
    putText(show_pic, dist, cv::Point(armor.center().x - 20, armor.center().y - 40), cv::FONT_HERSHEY_TRIPLEX, 0.8,color);
    sprintf(c_pitch, "predict_pitch=%.1f", pos_message.x);
    sprintf(c_yaw, "predict_yaw=%.1f", pos_message.y);
    putText(show_pic, c_pitch, cv::Point(armor.center().x - 20, armor.center().y - 60), cv::FONT_HERSHEY_TRIPLEX, 0.8,color);
    putText(show_pic, c_yaw, cv::Point(armor.center().x - 20, armor.center().y - 80), cv::FONT_HERSHEY_TRIPLEX, 0.8,color);
    sprintf(c_eular, "eul=%.1f %.1f %.1f", armor.box_pos.getrodetoEularVec()[0], armor.box_pos.getrodetoEularVec()[EULAR_AXIS],armor.box_pos.getrodetoEularVec()[2]);
    putText(show_pic, c_eular, cv::Point(armor.center().x - 300, armor.center().y - 30), cv::FONT_HERSHEY_TRIPLEX, 0.8,color);
    sprintf(c_real, "conf=%.1f", armor.getIDReal());
    putText(show_pic, c_real, cv::Point(armor.center().x - 150, armor.center().y), cv::FONT_HERSHEY_TRIPLEX, 0.8,color);
    //      else LOGE("Armor certexes volumn is not 4!")
}


